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Abstract: Sampling based planners have been successful in path planning of robots with 
many degrees of freedom, but still remains ineffective when the configuration space has a 
narrow passage. This paper presents two new techniques of preprocessing the configuration 
space. The first technique called a Random Walk to Surface (RWS), uses a random walk 
strategy to generate samples in narrow regions quickly, thus improving efficiency of Prob- 
abilistic Roadmap (PRM) based planners. The algorithm substantially reduces instances of 
collision checking and thereby decreases computational time. The method is powerful even 
for cases where the structure of the narrow passage is not known a priori, thus giving sig- 
nificant improvement over other known methods. The second method, by preprocessing the 
configuration space, improves the efficiency of Rapidly Exploring Random Tree (RRT) like 
planners by identifying key regions of the configuration space to search for a solution path. 
The Experiments show a significant improvement in efficiency for both PRM and RRT like 
planners. 
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1. INTRODUCTION 

Several areas beyond classical robotics, are benefiting 
from advances in algorithmic motion planning. Examples 
include structural studies in biology, computer games, path 
planning of unmanned aerial vehicles among the others. The 
success of these algorithms on different instances is because 
of availability of an abstract formulation of the model of 
many different real world applications, and ability to solve 
difficult problems in a reasonable time. Modern motion plan- 
ning begins with the introduction of the notion of configura- 
tion space by Lozano-Perez and Wesley [1], [2]. In the con- 
figuration space the robot is reduced to a point; hence the mo- 
tion planning problem becomes that of finding a path from an 
initial point to a goal point in the configuration space. How- 
ever, the explicit mapping from workspace obstacles to con- 
figuration space is in general difficult. Early studies showed 
that the basic version of this problem is PSPACE-complete 
[3], [4] [5] and the best exact deterministic algorithm known 
is exponential in the dimension of the configuration space. 
On the other hand, real world problems generate instances 
with high dimensional configuration spaces. 
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Since the mid nineties, in order to break this "curse of di- 
mensionality", sampling based approaches were introduced. 
The approach is based on the generation of samples to ac- 
quire information about the problem instance being solved. 
The first generation of these algorithms heavily relied on ran- 
dom samples. In contrast, there has been a recent trend to 
use deterministic sampling schemas [6]. For motion plan- 
ning problems, samples are stored in a data structure which 
represents an approximation of the configuration space, as 
opposed to its exact combinatorial representation. The data 
structure is usually composed of nodes, that is, samples in the 
configuration space, and links, that is, valid paths connecting 
samples. Nodes and links can be stored in the form of graphs 
or trees. The entire continuum of configuration space is then 
approximated to a network of nodes. The implementation of 
these algorithms is usually quite simple. The price to pay is 
completeness. 

Traditional combinatorial motion planning algorithms are 
complete, that is, they will find a solution if one exists, and 
will report failure otherwise. Algorithms based on randomly 
generated samples aims for probabilistic completeness. This 
means that if a solution exists, the probability to find it con- 
verges to 1 when the computation time approaches infinity 
[7]. Despite probabilistic completeness, randomized algo- 
rithms such as the Randomized Potential Field Planner (RPP) 
[8], the Probabilistic Road-map (PRM) family [9], Rapidly- 
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Exploring Random Trees (RRTs) [10], and others, have dom- 
inated the field of motion planning for over a decades. 

Despite the success of sampling based path planners, mo- 
tion planning in high dimensional configuration space is diffi- 
cult. Several instances of the problem have been even proven 
to be undecidable. It is unlikely that sampling, can overcome 
such difficulty entirely. For PRM like planners one such dif- 
ficulty arises when configuration space posses narrow pas- 
sages. Narrow passages are those critical regions in the con- 
figuration space that require a number of sample points in 
order to capture the free space connectivity. This difficulty 
posed by narrow passages and its importance were noted in 
early work on PRM planners [9] . Several sophisticated sam- 
pling strategies can remove this difficulty to a large extent, 
but a satisfactory answer remains elusive. Likewise, the is- 
sue of exploration vs. biasing issues in case of RRT poses 
a significant challenge. The 'rapid' nature of RRT is due to 
the fact that the samples pulls the tree towards the unexplored 
areas of configuration space. This leads to a creation of large 
number of unnecessary nodes before obtaining a final solu- 
tion. The exploration weightage and biasing weightage to- 
ward the goal is a critical design issue. 

In this paper we propose a new methods which is suitable 
to tackle problem of narrow passages in high dimensions for 
PRM. We also present a sequential sampling scheme which 
can guide an RRT effectively toward the goal state. In Sec- 
tion 2 we formulate the general motion planning problem. In 
Section 3 we briefly explain PRM method, problem of nar- 
row passage and related work. Section 4 describes our al- 
gorithm for narrow passage sampling. Section 5 discuss the 
implementation issues and result. Section 6 reports RRT al- 
gorithm and related exploration vs. biasing issues. Section 
7 gives the details of our proposed methodology to improved 
performance of RRT, while in subsequent sections we discuss 
and summarize the implementation issues and results. 

2. PROBLEM FORMULATION 

The configuration of a robot with n DOFs can be repre- 
sented as a point in an ^-dimensional space , called the con- 
figuration space C, which is locally like the d-Dimensional 
Euclidian Space R d . A configuration q is free if the robot 
placed at q does not collide with the obstacles or with itself. 
We define the free space Cf ree to be the set of all free con- 
figurations in C There are a finite number of obstacles in 
the configuration space, which are closed bounded sets Ou 
i = 1, 2, • • •, m and we can fairly assume that they are pair- 
wise disjoint. Let the starting configuration be x start e Cf ree 
and the final configuration be x goa \ e Cf ree - For convergence 
issues we define a rather general goal subset X goa u than a spe- 
cific point. Clearly Xf ree e X goa \. The motion planning prob- 
lem is to find a path connecting x start with X goa u that is, a 
continuous function / : [0, 1] — > Cf ree such that /(0) = x start 
and /(l) e X goa i. We assume an appropriate collision checker 
function does exist for instance of the problem. In related 
literature the configuration space C is frequently substituted 
with the state space X, which includes both the degrees of 



freedom and their derivatives. The constraints can be non- 
holonomic and differential. 



3. PRM AND NARROW PASSAGE PROBLEM 

Probabilistic road-map methods solves motion planning 
problems that do not involve dynamics of the robot or have 
negligible dynamics. A classic multi-query PRM planner 
proceeds in two stages. In the first stage, it randomly chooses 
samples from Cfree, called milestones according to a sam- 
pling scheme( which in general uniform). It then uses these 
milestones as nodes to construct a graph, called a road-map, 
by adding an edge between every k pair of milestones that 
can be connected via a simple collision-free path, typically, 
a straight-line segment. After the road-map has been con- 
structed, multiple queries can be answered quickly in the sec- 
ond stage. Each query consists of an initial configuration and 
a goal configuration, and asks for a collision-free path con- 
necting x start and x goa i. The planner first finds two milestones 
in the road-map, such that x star t and x goa \ can be connected to 
these nodes, respectively. The rest of the job is to search for 
a path (Or may be search for the shortest path). 

If the configuration space possess a narrow passage, then 
to capture the connectivity of C/y ee , it is essential to sample 
milestones in narrow passages. This, however, is difficult, 
because of small volumes of narrow passages. Any volume- 
based sampling distribution is likely to fail. Uniform dis- 
tribution may not work well when the dispersion of the sam- 
ples is higher than the narrow passage volumes. Furthermore, 
when dealing with multiple degree of freedom robots, we do 
not have an explicit representation of Cf ree and cannot locate 
narrow passages directly by processing the global geometry 
of Cfree- Intuitively, one can sample more densely near ob- 
stacle boundaries because points in narrow passages lie close 
to obstacles. This method called Gaussian sampler [11] is a 
simple and efficient algorithm that uses this idea. However, 
in some cases, many points near the obstacle boundaries lie 
far away from narrow passages and do not help in improving 
the connectivity of road-maps. So, despite the improvements, 
sampling near obstacle boundaries may generate many sam- 
ples in uninteresting regions. Other geometric approaches 
[12], [13], [14] also exist but those are expensive to imple- 
ment in high-dimensional configuration spaces. 

Perhaps the most appealing scheme to answer the problem 
of narrow passage is the Bridge test [15]. Here when a 
sample lies within an obstacle, one uses this information to 
build a bridge whose two end points lie in the obstacle while 
the mid point lies in Cf ree . This method, although it requires 
a high computational time (because of more number of 
collision checking), can be very effective. In the following, 
we give the standard bridge test algorithm (Randomized 
Bridge Builder). 
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Algorithm: Randomized Bridge Builder (RBB) 

For i <- 1 to K 

q\ <— Random Configuration(Umform Distribution) 
If Clearance (q\) returns False 
then ^2 <— Random Configuration^) 
If Clearance (qi) returns False 
then q 3 <— mid point of qiq2 
If Clearance^ returns True 
then Insert q^ to List G as a new milestone 
Return(G) 

Here "Clearance" is a collision checker which returns true 
if the sampled configuration is in Cf ree and A x is a multi- 
dimensional Gaussian distribution with a pre-specified vari- 
ance cr. The performance of this algorithm depends heavily 
on the choice of cr. If one chooses cr to be very small it takes 
a very long time and numerous collision checking before to 
come up with a point in Cf ree - Also a large cr results in large 
number of redundant configurations. The computation time 
also increases with the increase in number of obstacles. 

4. RANDOM WALK TOWARD SURFACE 

Next we present our algorithm, which generates the 
sample points in the surface of the configuration space 
obstacle. The idea is, since in high dimension one does 
not know the suitable value of <x, and there is no a priori 
knowledge about narrow passage geometry, therefore the 
most idealistic approach will be to generate points on the 
surface of the obstacle. Note that in the Gaussian sampling 
scheme, one tries to generate points on the surface of the 
obstacles. But again a unsuitable value of cr for a particular 
problem instance may take a very long time to generate a 
point in the narrow passage. Our algorithm is based on a 
simple philosophy, such as, once a sample point is generated 
within the obstacle one can perform a discrete random walk 
with a fixed length size a, until the particle comes out of the 
obstacle. 




Algorithm: Random Walk Towards Surface (RWS) 

For i <- 1 to K 

q <— Random Configuration 

If q E Cfree 

then add to List L 
else Flag <— 1 
while Flag = 1 

> Select Random Direction 

q' <— Random Walk (q, p, a) 

If q' e Cfree 

then add q' to the list L 
Flag = 
else q <— q' 
Return L 

The most important aspect of the algorithm is the reduc- 
tion in collision checking computations. Both in RBB and 
Gaussian sampling strategy, to generate a point inside the ob- 
stacle, one has to check if the sampled random configuration 
belongs to either of m, OJs. This increases with increase in 
number of obstacles. In our algorithm, once a point is gen- 
erated inside obstacle 0\, it has to only check if the point 
belongs to only that obstacle O f { or not. This reduces compu- 
tation time drastically, as we shown on Table 1 . 

As in Gaussian sampling strategy, our algorithm gener- 
ates unnecessary points on the surface of the obstacle which 
may be far away from critical regions. We accept this as a 
disadvantage of the algorithm, and propose a discrimination 
method to eliminate unnecessary points. 

We define a heuristic distance d(x,y) oc i as threshold, 
where N is the number of points. Once all the points are 
generated, for every point p e N a neighbor list is created 
which contains points that lie within d(x, y). If all the neigh- 
bors originate from the same obstacle then p is deleted.Thus 
following algorithm describes the process 

Algorithm: Elimination(L) 

N ^Size(L) 
For / <- 1 to # 
Q <— Neighbors (q(i), d) 
I <— q(i). index 
M <- Size(Q) 
For k <- 1 to M 
if / = Q(k). index 
then p <— p + 1 
Up = M 
then delete q(i) 
else insert q(i) to List S 
Return S 



Figure 1 : RWS illustration. Shaded regions are obstacles 



This eliminates unnecessary points. We also try to provide 
an approximate analysis of RWS in terms of mean escape 
time r es required to come out of the obstacle. Although our 
proposed random walk is discrete, but we approximate this 
with a continuous time diffusion process. This allows us to 
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state a much more simple expression for mean escape time 
r es . Since the expression for mean exit time is available in 
existing literature, therefore we only state the result. For a 
somewhat elementary proof, see Appendix. A more general 
discussion and in depth analysis of diffusion process can be 
found in [16]. In particular, the expected exit time from a ball 
of radius R in d dimension is given by 



R z - \x[ 



x e 



(1) 



where x is the location of initial position of particle from ori- 
gin. Therefore, maximum of expected exit time is r max = ^-. 
Now the question is, how many steps will be taken up to 
and including time r max . We assume the waiting time be- 
tween successive steps is finite and constant. We denote this 
by ft. Therefore, the expected number of collision checking 
required before a particle comes out of domain Q is, 



N ( 



col 



d/3 



(2) 



Interestingly this indicates that as the dimension increases it 
becomes less and less probable for a random walker to return 
to the starting point as shown by Poly a [17]. This means par- 
ticles once sampled within the obstacles, are more probable 
to quickly appear onto the obstacle surface as the dimension 
goes high. 



Instance A 


RBB 


RWS 

after 
Elimination 


No of Points 


<x 


Time (Sec) 


Time (Sec) 


30 


2 


91.45 


11.7247 


Instance B 


RBB 


RWS 

after 
Elimination 


11 


2 


328.359 


9.4742 


5 


22.6167 


10 


8.2416 


20 


17.62 


Instance C 


RBB 


RWS 


14 


2 


165.862 


50.21331 


5 


26.52 


10 


15.03 


20 


3.144 


Instance D 


RBB 


RWS 


3 


2 


231.517 


9.823 


5 


29.02 


10 


17.124 


Instance E 


RBB 


RWS 


3 


2 


50.50 


9.823 


5 


5.281 


10 


1.27 



Table 1 : Comparison between RWS and RBB 



5. COMPARISON RESULTS BETWEEN RWS AND 
RBB 

We compare the samples generated in the critical region 
and time to generate them for both RBB and RWS algo- 
rithms in different critical environments, Fig 3 to Fig 8. We 
choose several benchmark problem, tested in a Pentium dual 
core processor and standard MATLAB runtime environment. 
Configuration space is [0, 100] x [0, 100]. In case of RWS 
the random walk has a fixed step size of 1 m. The compar- 
ison results in Table 1 shows the capability of RWS algo- 
rithm compared to RBB. Note that with increase of <x, RBB 
may generate samples not in the critical region as shown in 
Fig 7. The differences in the computation time with differ- 
ent <x clearly indicates the importance of proper <x selection. 
With increase in number of obstacles, computations become 
slower in case of RBB as shown in Fig 7 (which also evident 
from Table 1). 

6. RAPIDLY EXPLORING RANDOM TREE 

Rapidly Exploring Random Tree (RRT) have been shown 
to be very effective in solving robot motion planning 
problems in complex configuration space with kynodynamic 
constraints. RRT was introduced in [18], [10] as an efficient 
data structure and sampling scheme to quickly search high 
dimensional spaces that have algebraic constraints (arising 
from obstacle) and differential constraints (arising from 



nonholonomy and dynamics). The algorithm incrementally 
builds a tree whose nodes are different configurations of the 
robot/vehicle. The edges of the graph correspond to the 
feasible path between the configurations. The key idea of 
RRT is to bias the exploration toward unexplored portions 
of the space by sampling configurations, and incrementally 
pulling the search tree toward the unexplored portion [19]. 
In the following we present the basic RRT algorithm. 

Algorithm: Build-RRT(fe) 

T - init(qini t ) > Initialize tree T 
For k <- 1 to K do 

Qmnd <— Random Configuration 

Extend (T, q ra nd) 
Return T 



Extend (T, q) 

Qnear <- Nearest -Neighbor (q, T) 
tfnew <— New State (q ne ar, u) 
If Clearance (q ne w) is True 

then 

T • add-vertex(x new ) 

± ' aaa —ea^eyXyicciT ^ •^newi ^w£?w/ 

then Return Reached 



Proceedings of ICEAE 2009 



Titas Bera , M. Seetharama Bhat, D. Ghose 



else 
Return Advanced 
Return Trapped 

In the extend function, a nearest vertex in the tree to a given 
random sample state, is selected for future expansion. The 
function New- State makes a motion towards x by applying 
an input u e U to x new for some time increment At. This input 
can be chosen at random, or selected by trying all possible 
inputs and choosing the one which yields a new state as close 
as possible to the sample [10]. 



7. PREPROCESSING THE CONFIGURATION SPACE 

Many variants and modifications of the basic RRT algo- 
rithm can be found in [20], [21], [22], [23] etc, in order to 
address exploration vs biasing issues, nearest-neighborhood 
queries, and probabilistic convergence. In [24], it is men- 
tioned that different choice of metric in Cf ree can lead to 
a drastic difference in the performance of RRT. Apart from 
this issues one of the biggest problem is the issue of explo- 
ration vs biasing. While many used an artificial bias in gen- 
erating samples towards the goal [25], it is certainly not the 
best approach because of the potential problem of local min- 
ima. One variant is to pick up milestones in Probabilistic 
Roadmap method and try to push RRT to sequentially follow 
the PRM vertices [10]. But in general, for dynamical sys- 
tems, the problem of reachability may occur, and one never 
knows whether a desired milestone is reachable or not a pri- 
ori. In [20], [21] an adaptive biasing is used. There the focus 
is on the selection of the best among ^-nearest neighbors, 
and on adaptively increase or decrease the sampling bias. But 
again this approach depends on too many tuning parameters 
that depend on configuration space criticality. 

The RRT algorithm has a known exponential bound on 
its run length tail probabilities. Its nearest-neighbor opera- 
tion implies that individual iteration require increasing time. 
Therefore, it is reasonable to assume that run time tail prob- 
abilities may arise that are heavier than exponential. It is 
shown in [26], that two independent forward search RRT, 
solving the same query, can reach roughly the same level of 
progress at drastically different run lengths. 

So far there is little proof on probabilistic completeness of 
RRT for arbitrary kino-dynamic problem. In [10], a theorem 
states that for both convex and non-convex state space and 
for holonomic path planing problem, the probability, that the 
dispersion of the RRT vertices will be less than an arbitrary 
positive threshold, will converge to 1 as the number of 
vertices approaches infinity. 

Theorem [10]: Suppose Xi n u and x goa \ lie in the same 
connected components of a convex/non-convex, bounded 
open n dimensional state space. The probability that an RRT 
constructed from X[ n i t will find a path to x goa \ approaches one 
as the number of RRT vertices approaches infinity. 



Based on the above theorem, we realize that, identifica- 
tion of necessary segments of the configuration space (state 
space) that is worth searching sequentially for the goal, may 
generate a lower dispersion value for a given number of RRT 
vertices, that is, the solution can be found 'quickly' . This mo- 
tivates us to pre-process the configuration space to find most 
suitable regions to be sequentially explored before becoming 
close enough to x goa \. As an analogy, imagine RRT to be 
a big ship approaching a harbor and requiring the guidance 
of a small pilot ship which knows the different channels to 
approach having different quality measures. An RRT used 
for kinodynamic problem therefore requires a set of guid- 
ing milestones representing the key segments. These guiding 
milestones cannot be chosen using elementary PRM, because 
those may not be reachable from the current configuration. 
Before describing our approach, we define the following: 

1. State Space: A topological space X e [0, l] n . 

2. Boundary Value: xi n i t e X and X goa \ c X. 

3. Collision Detector: A function, D : X — > {true, false) 
that determines whether global constraints are satisfied 
for state x. 

4. Inputs: A set U which specifies the complete set of con- 
trols. 

5. Incremental Simulator A(nAt) : Given the current state 
x(t) and inputs applied over a time interval nAt, the in- 
cremental simulator computes x(t + rjAt). 

6. Metric: A real valued function p :IxI^ [0,oo]. 

7. Reachable set: 

t k +rjAt 

R(x\ t\ nAt) = {x<=X\u<=U,x = x k + f f(x, u)dt 

t k 

8. Voronoi partition of set P: Let P := {p\,pi, ••••Pn) be a 
set of n distinct points in W 1 ; these points are the sites. 
The voronoi diagram of P as the subdivision of the space 
into n cells, one for each site in P, with the property that 
a point q lies in the cell corresponding to a site pi if an 
only if p(q, p t ) < p(q, pj) for each pj e P with j ± i. 
The voronoi partition of set P is V(P), Fig(2). 

We assume no a priori knowledge about the configuration 
space (state space). By uniform sampling scheme we choose 
N number of points in the free portions of the state space. 
The goal is to generate a non-uniform partition of the space. 
The suitable value of N of course depends on the dimension 
and range of the problem instance. Next, we do a Voronoi 
partition of the space based on these N registration points 
(sites), V(P), according to the given metric p. The goal is to 
find a single step transition probability of a particle from re- 
gion i to region j of the Voronoi partition. One problem may 
occur that, once we partitioned the state space, connectivity 
of a region or cell may be lost. Since the shape of the config- 
uration space obstacle is unknown, the lowest grid resolution 
required to overcome the problem is also unknown. Only we 
can assume that the C-space obstacles OJs having large mea- 
sure are more likely to create problems for a specific N. It is 
clear that points on the surface of the obstacle generated in an 
uniform manner can reduce the problem of disconnectedness 
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sponding cells) we need to define the quantity 
6 t (i) = max P[q u q 2 , ..., q t = i] 

qi,q 2 ,...,q t -i 



(3) 



that is, 5 t (i) is the best score along a single path at stage t 
which ends at cell St. By induction 



S t+ i(J) = ma.xd t (i)P ij 



(4) 



To retrieve the cell sequence, we need to keep track of the 
argument that maximized (4), for each t and j. We do this via 
the array if/(j). The complete procedure for finding the best 
state sequence can now be stated as 



Figure 2: Voronoi Partition of space 



substantially. We use therefore, the RWS algorithm to find 
points on the surface of the obstacles. We call this as enrich- 
ment of the initial Voronoi cell distribution. The degree of 
reduction is dependent on the number M of additional points 
generated. 

We do a repartitioning using Voronoi grid of N + M points. 
Next, using Monte-Carlo method we approximate the vol- 
ume measure of the free portions of Voronoi grid cells, with 
a confidence level of 0.95 [27]. We generate N' particles in 
Cfree by uniform distribution. 

Once we have an approximate measure of the free volume 
in each cell Wu we give random excitation input to the indi- 
vidual particle for a time period of r/At. The incremental sim- 
ulator gives us the position of the particle within a Voronoi 
cell. We then calculate the transition probability P^-, defined 
as the probability that the particle at cell i at time t, will go 
to cell j at time t + r]At. For all such grid cell we get the r] 
step transition probability matrix P n . Assuming the time in- 
terval T = rjAt, we rename this P r] as single step transition 
probability P/ 7 . Note that for the transition probability calcu- 
lation, the underlying assumption of Markovian nature of the 
process exists. So 



Pu = 



Number of particles that goes into cell j from cell , 
Number of particles fall inside cell i 



N+M 



1 V/e [1,2,..., N + M] 



Once we have the transition probability of the entire grid 
regions we can calculate, such as, I st passage times from the 
Celli containing the initial configuration to Cell g containing 
final configuration and most probable path y from Celli to 
Cell g . Assuming one such path exists, the most probable path 
y m has the maximum probability over all such paths. 

Next, we describe the dynamic programming procedure to 
find y m for a given V(P N+M ). 

Algorithm for Most Probable Path: To find the single 
best cell sequence Q = {q\,q2, ••-, qr} (q\,qi, •••, qr are corre- 



1. 


Initialization 








*i(0 = 


7TiWi 


(5) 




<M0 = 





(6) 


2. 


Recursion 








StU) = 


max 6f-i(i) aijWi 

l<j<N J 


(7) 




Ui) = 


argmax 8 t -\(f) OijWi 

\<i<N 


(8) 


3. 


Termination 








p* — 


max 8t(i) 

l<i<N 


(9) 




* 
q T = 


argmax 8 T (i) 

l<i<N 


(10) 


4. 


Back Trackin 


g 





q* = iff t +i(q* t+ i) t = T-l,T-2, ..., 1 



(11) 



It is important to choose the number of stages T over which 
the maximization requires to be done. One can evaluate first 
passage characteristic from cell i to cell j and compute the 
mean first passage time. There are standard Monte Carlo 
based procedures that exist for finding approximate mean 
first passage time T [27] . The interpretation of the most prob- 
able sequence cell is that these zones are to be sequentially 
searched by RRT. The Voronoi registration points of these 
cells are the guiding milestones. 



8. IMPLEMENTATION ISSUES AND RESULTS 

Although there is no guarantee that every time the above 
pre- processing of configuration space lead to the best se- 
quence path (zone). The procedure clearly depends on 

1. The non-uniform grid distribution. 

2. The number of grid cells. 

3. The number of points generated on the surface of the 
obstacles. 

4. The criticality of the configuration space. 

5. The reachable set R k . 
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While implementing one should carefully evaluate the tran- 
sition probability matrix, as it is the heart of the whole pro- 
cedure. Depending on grid distribution one may find cases 
where Py = 1 which indicates a transient region. There one 
may generate additional points with increased r]At and try to 
approximate Py. Also, there may be cases where Py = 
because of small cell measure. In this case one can excite 
the corresponding cell registration point with random control 
input and thus approximate the value of Pfj. Depending on 
the configuration space criticality there may be large uncer- 
tainty in evaluating transition probability matrix. Inclusion 
of uncertainty in P and generating a robust procedure for the 
method is our future concern. We have tested our method 
for path planning of nonholonomic mobile robots, although 
it can be applied to any other configuration space as well. 
The vehicle dynamics is defined by 



x = v cos(#) 
y = v sin(#) 
6 = (v/L) tan(0) 



(12) 
(13) 
(14) 



The configuration space is R 2 x S 1 . We intentionally created 
6 critical obstacle traps so that a conventional RRT requires 
much longer time to come to the goal region. For each of 
the 6 problem instances a most probable path is found by our 
algorithm. Once the sequential milestones are available one 
can guide the RRT using biased sampling towards the mile- 
stones. We used 3000-5000 samples to approximate the cell 
free volume and to calculate the transition probability. For 
problem instances A to E (Fig [9-13]) the start configura- 
tion is (50,50,0) and goal configuration is (100,100,20) and 
for problem instance F in Fig 14, the start configuration is 
(0,0,0) while the goal configuration is (100,100,0). A limited 
number (usually 10-100, depending on the problem instance) 
of initial Voronoi cells (N Vor ) are created. Enrichment N^ de- 
pends on the criticality of the problem instance. One can ar- 
gue about the added computation required for this approach, 
but clearly the results shows it is worth investing in the ex- 
treme case. 

9. CONCLUSIONS AND FUTURE WORK 

We developed configuration space preprocessing tech- 
niques to improve the efficiency of sampling based planners 
like PRM and RRT in critical situation. The approaches are 
promising. In future our focus will be on forming a robust ap- 
proach to tackle uncertainty in transition probability matrix 
calculation and consequently development of an improved 
pre-processing technique. 
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APPENDIX 

We begin by assuming a random sample generated within a 
obstacle and calculate maximum of mean exit time max(r^). 
This is equivalent to evaluate the maximum mean exit time 
from a domain Q, of a particle, which is showing brownian 
motion in a stationary fluid medium. If b(t, x) e R 3 is the 
velocity of the fluid at the point x at time t, then a reasonable 
mathematical model for the position X t of the particle at time 
t would be a stochastic differential equation of the form 



^-=b(t,X t ) + <r(t 9 X t )W t 
at 



(15) 



where, W t e 
equation is 



is a white noise. The ltd interpretation of this 



dX t = b{t, X t )dt + cr(t, X t )dB t 



(16) 



where B t is m-dimensional Brownian motion, X t e R , 
b(t,X t ) e R d , cr(t,X t ) e R dxm . We call b the drift coefficient 
and cr the diffusion coefficient. 

We can associate a second order partial differential opera- 
tor A to an ltd diffusion X t . The basic connection between A 
and X t is that A is the generator of the process X t . Let {X t } 
be a time-homogeneous ltd diffusion in R^. The infinitesimal 
generator A of X t is defined by 



A f(x) 



lim 
*->o 



E x [fiX t )]-fix) 



t 



(17) 



where, x e R d , X t is the solution of ltd diffusion, / : R^ -> R 
such that limit exists at x. E x define expectation of x with 
respect to probability measure. 

Consider the stationary case of ltd diffusion when b = 
and cr = I d i.e., 



dX t = dB t 
The generator of B t is 



v-kZ* f 



(18) 



(19) 



2 ^ dx t 2 

where, / = f(x\,X2, ...., xj) twice differentiable, that is, A = 
^A where A is the Laplace operator. 

Now we state Dynkin's Theorem [16]: Let / is a twice dif- 
ferentiable function. Suppose r is the stopping time (escape 
time from a domain Q, and E x [r] < oo) then 



E x [fiX T )] = fix) + E x [ AfiX s )ds] 



■■/' 



(20) 



According to the Dynkin's theorem we choose fix) = \x\ 2 
for \x\ < R, X = B and r = cr^ = min(^, r K ), where k is any 
integer. So, 



E^fiB^)] 



fia) + E a [J^AfiB s )ds] 



o 



= \a\ 2 + E a [ d-ds] 



f' 



= \a\ 2 + d ■ E a [cr k ] 
Letting k — > oo, and t^ = lim cr^ < oo a. s. and 

E a [T k ] = l -(R 2 - \a\ 2 ) 
a 



(22) 

(23) 
(24) 

(25) 



Now consider a Brownian motion B = iB\,B2, ...., Bd) start- 
ing at a = iai,d2, ..., a^) e R^ and assume \a\ < R. We define 
the first exit time as i> of B from the ball 



K R = {x e R d ; \x\ < R] 



(21) 
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Figure 3: Problem Instance A (a) RWS without elimination (b) RWS with 
elimination (c) RBB (<x = 2) 
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Figure 5: Problem Instance C (a) RWS without elimination (b) RWS with 
elimination (c) RBB (cr = 20) (d) RBB (a = 10) (e) RBB (cr = 5) (f) RBB 



(a) 



(b) 



fl 



fl 



20 40 60 80 



n 



*..k 



D 
fl 



(c) 



(d) 



Figure 4: Problem Instance B (a) RWS without elimination (b) RWS with 
elimination (c) RBB (cr = 2) (d) RBB (cr = 5) 
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Figure 6: Problem Instance D (a) RWS without elimination (b) RBB (cr = 2) 
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Figure 7: Problem Instance D (a) RBB (cr = 2) (b) RBB (cr = 10) 
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Figure 8: Problem Instance E (a) RWS without elimination (b) RBB (cr = 2) 
(c) RBB (o- = 5) 





Figure 10: Problem instance B: N grid = 100, N n = 30, 5000 samples 
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Figure 11: Problem instance C: N gr id = 10, N n = 50, 3000 samples 
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Figure 9: Problem instance A: N gr id = 20, N n = 100, 3000 samples 



Figure 12: Problem instance D: N gr td = 10, N n - 100, 5000 samples 
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Figure 13: Problem instance E: N gr td = 10, N n = 100, 3000 samples 
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Figure 14: Problem instance F: N gr id - 10, N v = 30, 3000 samples 
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